variables
N=(tend-t0)/stepsize;
t=t0:stepsize:tend;

u=chirp(t,f0,tend,tend*omega/pi);
% CHIRP  Swept-frequency cosine generator.
%    Y = CHIRP(T,F0,T1,F1) generates samples of a linear swept-frequency
%    signal at the time instances defined in array T.  The instantaneous
%    frequency at time 0 is F0 Hertz.  The instantaneous frequency F1
%    is achieved at time T1.  By default, F0=0, T1=1, and F1=100.

x0=[0;0;0];
x=zeros(length(Phi),N);
x(:,1)=x0;
y=zeros(N,1);

for k=1:N
    prev=random('Normal',0,1);
    prew=[random('Normal',0,1,2,1)]; % Standardnormalverteilter Zufallsvektor mit Dimension 2x1
    v=preR*prev;
    w=preQ*prew;
    x(:,k+1)=Phi*x(:,k)+ Gamma*u(k) + G*w;
    y(k)=C*x(:,k) + D*u(k) + H*w + v;
end

K=zeros(N,1); % K aus R(ixj), i=Anzahl Eintraege von x, j=anzahl eintraege von y.
xhat=zeros(length(Phi),N); %dim(xhat) = dim(x)
P=zeros(length(Phi),length(Phi),N); %P(i) sind quadratisch


for k=1:N
    K = (Phi*P(:,:,k)*C' + G*Q*H.')*((C*P(:,:,k)*C'+H*Q*H'+R)^(-1));
    xhat(:,k+1) = Phi*xhat(:,k) + Gamma*u(k) + K*(y(k)-C*xhat(:,k)-D*u(k));
    P(:,:,k+1) = Phi*P(:,:,k)*Phi' + G*Q*G' - K*(C*P(:,:,k)*Phi' + H*Q*G.');
end

plot(xhat(1,:))
%plot(xhat(2,:))
%plot(xhat(3,:))